#include "CarController.h"

CarController::~CarController()
{
}

void CarController::publish()
{
    double delta_last_msg_time = ros::Time::now().toSec() - this->lastMsg.toSec();
    //如果超时
    if(delta_last_msg_time > this->timeout.toSec()){
        ROS_INFO("Over Time: delta_last_msg_time[%f] > timeout[%f]", delta_last_msg_time, this->timeout.toSec());
        this->x = 0;
        std_msgs::Float64 msgRear = std_msgs::Float64();
        msgRear.data = this->x;
        this->pub_rearL.publish(msgRear);
        this->pub_rearR.publish(msgRear);
        this->z = 0;
        std_msgs::Float64 msgSteer = std_msgs::Float64();
        msgSteer.data = this->z;
        this->pub_steerL.publish(msgSteer);
        this->pub_steerR.publish(msgSteer);
        return;
    }

    //如果没有超时，但是转角很小，即直行
    if(fabs(this->z) < 0.000001){
        ROS_INFO("Go Straight: x[%f] z[%f]", this->x, this->z);
        std_msgs::Float64 msgRear = std_msgs::Float64();
        msgRear.data = this->x;
        this->pub_rearL.publish(msgRear);
        this->pub_rearR.publish(msgRear);
        std_msgs::Float64 msgSteer = std_msgs::Float64();
        msgSteer.data = this->z;
        this->pub_steerL.publish(msgSteer);
        this->pub_steerR.publish(msgSteer);
    }else{
        //如果是转弯
        //转弯半径
        ROS_INFO("Turn: x[%f] z[%f]", this->x, this->z);
        double r = this->L / fabs(tan(this->z));
        double sign = this->z > 0 ? 1.0 : -1.0;
        double rL_rear = r - sign * (this->T_rear / 2.0);
        double rR_rear = r + sign * (this->T_rear / 2.0);
        double rL_front = r - sign * (this->T_front / 2.0);
        double rR_front = r + sign * (this->T_front / 2.0);

        std_msgs::Float64 msgRearR = std_msgs::Float64();
        std_msgs::Float64 msgRearL = std_msgs::Float64();
        msgRearR.data = this->x * rR_rear / r;
        msgRearL.data = this->x * rL_rear / r;
        this->pub_rearR.publish(msgRearR);
        this->pub_rearL.publish(msgRearL);

        std_msgs::Float64 msgSteerL = std_msgs::Float64();
        std_msgs::Float64 msgSteerR = std_msgs::Float64();
        msgSteerL.data = sign * atan(this->L / rL_front);
        msgSteerR.data = sign * atan(this->L / rR_front);
        this->pub_steerL.publish(msgSteerL);
        this->pub_steerR.publish(msgSteerR);
    }
}

void CarController::on_cmd_vel(const geometry_msgs::Twist &msg)
{
    this->x = msg.linear.x / 0.3;
    this->z = this->maxsteer < msg.angular.z ? this->maxsteer : msg.angular.z;
    this->z = this->z > -this->maxsteer ? this->z : -this->maxsteer;
    //this->z = max(-this->maxsteer, min(this->maxsteer, msg.angular.z));
    //ROS_INFO("I heard: x[%f] z[%f]", this->x, this->z);
    this->lastMsg = ros::Time::now();
}

void CarController::update_odom(){
    //待发送的四元数数据
    geometry_msgs::Quaternion send_orientation = geometry_msgs::Quaternion();
    //tf广播者待发送数据
    geometry_msgs::TransformStamped trans = geometry_msgs::TransformStamped();
    //base_link坐标
    double x_base_link = 0;
    double y_base_link = 0;
    double z_base_link = 0;
    //各分量速度
    double Vx = 0;
    double Vy = 0;
    double Vz = 0;
    ros::Time now = ros::Time::now();
    //处理定位数据
    //获取四元数、读取base_link三轴酵素读，读取base_link三轴加速度信息
    

    //发布TF变换

    //发布odom里程计信息
    nav_msgs::Odometry odom = nav_msgs::Odometry();
    odom.header.stamp = now; //时间戳
    odom.header.frame_id = "odom"; //里程计坐标名称
}


void CarController::odom_cb(const geometry_msgs::TwistStamped &msg){

    //创建一个tf变换广播者
    static tf::TransformBroadcaster odom_br;

    //采集订阅到的车辆速度信息
    double vx = msg.twist.linear.x;
    double vy = msg.twist.linear.y;
    double vth = msg.twist.angular.z;

    //记录时间变化量
    this->current_odom_time = ros::Time::now();
    double dt = (this->current_odom_time - this->last_odom_time).toSec();
    
    //计算速度变化量
    double delta_x = (vx * cos(this->odom_th) - vy * sin(this->odom_th)) * dt;
    double delta_y = (vx * sin(this->odom_th) + vy * cos(this->odom_th)) * dt;
    double delta_th = vth * dt;

    //位移积分
    this->odom_x += delta_x;
    this->odom_y += delta_y;
    this->odom_th += delta_th;
    
    //将yaw旋转分量转化为四元数
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(this->odom_th);

    //tf广播者待发送数据
    geometry_msgs::TransformStamped trans = geometry_msgs::TransformStamped();
    trans.header.stamp = ros::Time::now(); //tf变换时间戳
    trans.header.frame_id = "odom"; //发生变换的link
    trans.child_frame_id = "base_link"; //基准link

    //odom坐标信息
    trans.transform.translation.x = this->odom_x;
    trans.transform.translation.y = this->odom_y;
    trans.transform.translation.z = 0.0;
    trans.transform.rotation = odom_quat;
    //发布odom坐标
    odom_br.sendTransform(trans);

    //发布odom话题
    nav_msgs::Odometry odom;
    odom.header.stamp = this->current_odom_time;
    odom.header.frame_id = "odom";
 
    //set the position
    odom.pose.pose.position.x = this->odom_x;
    odom.pose.pose.position.y = this->odom_y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;
 
    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    this->odom_pub.publish(odom);

    //将当前时间记录为历史时间
    this->last_odom_time = this->current_odom_time;
}
